#include "xyzmodule.h"

XYZModulePos::XYZModulePos() : XYModulePos()
{
    setOptionalProperty("safetyMoveSequence", toVariantList(SafetyMoveSequenceEnumInfo().names()));
    init();
}

XYZModuleConfig::XYZModuleConfig(QObject *parent) : AxisModuleConfig(&XYZModulePos::staticMetaObject, parent)
{
    init();
}

XYZModule::XYZModule(QString name, XYZModuleConfig *xyzModuleConfig, QObject *parent)
    : AxisModule(xyzModuleConfig, parent), xyzModuleConfig(xyzModuleConfig)
{
    setObjectName(name);
    MotionElementContainer::getIns()->registerItem<XYZModule>(name, this);
}

void XYZModule::moveToWithOffset(QString posName,
                                 XYZModulePos::SafetyMoveSequence safetyMoveSequence,
                                 double xOffset,
                                 double yOffset,
                                 double zOffset,
                                 bool waitDone,
                                 bool logIt)
{

    QString safetyMoveSeq = XYZModulePos::SafetyMoveSequenceEnumInfo().enumToName(safetyMoveSequence);
    XYZModulePos *pos = xyzModuleConfig->getPos<XYZModulePos>(posName);
    if (m_xAxis->isAtPos(pos->xPos() + xOffset, pos->xPrecision())
        && m_yAxis->isAtPos(pos->yPos() + yOffset, pos->yPrecision())
        && m_zAxis->isAtPos(pos->zPos() + zOffset, pos->zPrecision()))
    {
        return;
    }
    if (logIt)
    {
        qCInfo(motionCate(), N_FUNC_ARG(posName, safetyMoveSeq, xOffset, yOffset, zOffset));
    }

    auto groups = safetyMoveSeq.split("_");
    for (int i = 0; i < groups.length(); i++)
    {
        QString group = groups[i];
        QList<AxisTargetInfo> axesTargetInfo;
        for (int j = 0; j < group.length(); j++)
        {
            auto axisName = group[j];
            if (axisName == "S")
            {
                if (axesTargetInfo.count() == 0)
                {
                    throw SilicolAbort(tr("Unkown safetyMoveSeq %1(%2, %3)").arg(safetyMoveSeq).arg(i).arg(j),
                                       EX_LOCATION);
                }
                auto lastAxisName = group[j - 1];
                if (lastAxisName == "X")
                {
                    if (!xyzModuleConfig->useXSafetyPos())
                    {
                        throw SilicolAbort(
                            tr("Try to move %1 x to safety pos, but useXSafetyPos is false!").arg(name()), EX_LOCATION);
                    }
                    axesTargetInfo[axesTargetInfo.count() - 1].targetPos = xyzModuleConfig->xSafetyPos();
                }
                else if (lastAxisName == "Y")
                {
                    if (!xyzModuleConfig->useYSafetyPos())
                    {
                        throw SilicolAbort(
                            tr("Try to move %1 y to safety pos, but useYSafetyPos is false!").arg(name()), EX_LOCATION);
                    }
                    axesTargetInfo[axesTargetInfo.count() - 1].targetPos = xyzModuleConfig->ySafetyPos();
                }
                else if (lastAxisName == "Z")
                {
                    if (!xyzModuleConfig->useZSafetyPos())
                    {
                        throw SilicolAbort(
                            tr("Try to move %1 z to safety pos, but useZSafetyPos is false!").arg(name()), EX_LOCATION);
                    }
                    axesTargetInfo[axesTargetInfo.count() - 1].targetPos = xyzModuleConfig->zSafetyPos();
                }
                else
                {
                    throw SilicolAbort(tr("Unkown safetyMoveSeq %1(%2, %3)").arg(safetyMoveSeq).arg(i).arg(j),
                                       EX_LOCATION);
                }
            }
            else
            {
                if (axisName == "X")
                {
                    axesTargetInfo.append(AxisTargetInfo(m_xAxis, pos->xPos() + xOffset, pos->xPrecision()));
                }
                else if (axisName == "Y")
                {
                    axesTargetInfo.append(AxisTargetInfo(m_yAxis, pos->yPos() + yOffset, pos->yPrecision()));
                }
                else if (axisName == "Z")
                {
                    axesTargetInfo.append(AxisTargetInfo(m_zAxis, pos->zPos() + zOffset, pos->zPrecision()));
                }
                else
                {
                    throw SilicolAbort(tr("Unkown safetyMoveSeq %1(%2, %3)").arg(safetyMoveSeq).arg(i).arg(j),
                                       EX_LOCATION);
                }
            }
        }
        foreach (auto axisTargetInfo, axesTargetInfo)
        {
            axisTargetInfo.axis->absMove(axisTargetInfo.targetPos, false);
        }
        if (i < groups.length() - 1 || waitDone)
        {
            foreach (auto axisTargetInfo, axesTargetInfo)
            {
                axisTargetInfo.axis->waitArrivedPos(axisTargetInfo.targetPos, axisTargetInfo.precision);
            }
        }
    }
}

void XYZModule::waitArrivedPosWithOffset(QString posName, double xOffset, double yOffset, double zOffset)
{
    XYZModulePos *pos = xyzModuleConfig->getPos<XYZModulePos>(posName);
    m_xAxis->waitArrivedPos(pos->xPos() + xOffset, pos->xPrecision());
    m_yAxis->waitArrivedPos(pos->yPos() + yOffset, pos->yPrecision());
    m_zAxis->waitArrivedPos(pos->zPos() + zOffset, pos->zPrecision());
}

void XYZModule::relMove(double xStep, double yStep, double zStep)
{
    if (!qFuzzyCompare(xStep, 0))
    {
        m_xAxis->relMove(xStep, false);
    }
    if (!qFuzzyCompare(yStep, 0))
    {
        m_yAxis->relMove(yStep, false);
    }
    if (!qFuzzyCompare(zStep, 0))
    {
        m_zAxis->relMove(zStep, false);
    }
    if (!qFuzzyCompare(xStep, 0))
    {
        m_xAxis->waitArrivedPos();
    }
    if (!qFuzzyCompare(yStep, 0))
    {
        m_yAxis->waitArrivedPos();
    }
    if (!qFuzzyCompare(zStep, 0))
    {
        m_zAxis->waitArrivedPos();
    }
}

void XYZModule::moveXYToPos(QString posName, bool waitDone)
{
    qCInfo(motionCate(), N_FUNC_ARG(posName));

    XYZModulePos *pos = xyzModuleConfig->getPos<XYZModulePos>(posName);
    m_xAxis->absMove(pos->xPos(), false);
    m_yAxis->absMove(pos->yPos(), false);
    if (waitDone)
    {
        m_xAxis->waitArrivedPos(pos->xPos(), pos->xPrecision());
        m_yAxis->waitArrivedPos(pos->yPos(), pos->yPrecision());
    }
}

void XYZModule::moveZToPos(QString posName, bool waitDone)
{
    qCInfo(motionCate(), N_FUNC_ARG(posName));

    XYZModulePos *pos = xyzModuleConfig->getPos<XYZModulePos>(posName);
    m_zAxis->absMove(pos->zPos(), false);
    if (waitDone)
    {
        m_zAxis->waitArrivedPos(pos->zPos(), pos->zPrecision());
    }
}

void XYZModule::waitXYArrivedPos(QString posName)
{
    XYZModulePos *pos = xyzModuleConfig->getPos<XYZModulePos>(posName);
    m_xAxis->waitArrivedPos(pos->xPos(), pos->xPrecision());
    m_yAxis->waitArrivedPos(pos->yPos(), pos->yPrecision());
}

void XYZModule::waitZArrivedPos(QString posName)
{
    XYZModulePos *pos = xyzModuleConfig->getPos<XYZModulePos>(posName);
    m_zAxis->waitArrivedPos(pos->zPos(), pos->zPrecision());
}

void XYZModule::moveXyToPosWithOffset(QString posName, double xOffset, double yOffset, bool waitDone)
{
    qCInfo(motionCate(), N_FUNC_ARG(posName, xOffset, yOffset));

    XYZModulePos *pos = xyzModuleConfig->getPos<XYZModulePos>(posName);
    m_xAxis->absMove(pos->xPos() + xOffset, false);
    m_yAxis->absMove(pos->yPos() + yOffset, false);
    if (waitDone)
    {
        m_xAxis->waitArrivedPos(pos->xPos() + xOffset, pos->xPrecision());
        m_yAxis->waitArrivedPos(pos->yPos() + yOffset, pos->yPrecision());
    }
}

void XYZModule::waitXyArrivedPosWithOffset(QString posName, double xOffset, double yOffset)
{
    XYZModulePos *pos = xyzModuleConfig->getPos<XYZModulePos>(posName);
    m_xAxis->waitArrivedPos(pos->xPos() + xOffset, pos->xPrecision());
    m_yAxis->waitArrivedPos(pos->yPos() + yOffset, pos->yPrecision());
}

void XYZModule::moveXY(double xTarget, double yTarget, double precision)
{
    qCInfo(motionCate(), N_FUNC_ARG(xTarget, yTarget, precision));

    m_xAxis->absMove(xTarget, false);
    m_yAxis->absMove(yTarget, false);
    m_xAxis->waitArrivedPos(xTarget, precision);
    m_yAxis->waitArrivedPos(yTarget, precision);
}

void XYZModule::moveToPos(QString posName, XYZModulePos::SafetyMoveSequence safetyMoveSequence, bool waitDone)
{
    qCInfo(motionCate(), N_FUNC_ARG(posName, safetyMoveSequence));

    moveToWithOffset(posName, safetyMoveSequence, 0, 0, 0, waitDone, false);
}

QVariantMap XYZModule::getModuleCurrentPos()
{
    QVariantMap axesCurrentPos;
    axesCurrentPos["xPos"] = m_xAxis->getFeedbackPos(-1);
    axesCurrentPos["yPos"] = m_yAxis->getFeedbackPos(-1);
    axesCurrentPos["zPos"] = m_zAxis->getFeedbackPos(-1);
    return axesCurrentPos;
}

void XYZModule::initImpl()
{
    m_xAxis = MotionElementContainer::getIns()->getItem<SCAxis>(xyzModuleConfig->xAxisName());
    m_yAxis = MotionElementContainer::getIns()->getItem<SCAxis>(xyzModuleConfig->yAxisName());
    m_zAxis = MotionElementContainer::getIns()->getItem<SCAxis>(xyzModuleConfig->zAxisName());
}

void XYZModule::moveTo(QString posName, bool waitDone)
{
    auto pos = xyzModuleConfig->getPos<XYZModulePos>(posName);
    auto safetyMoveSeq = XYZModulePos::SafetyMoveSequenceEnumInfo().nameToEnum(pos->safetyMoveSequence());
    moveToPos(posName, safetyMoveSeq, waitDone);
}

void XYZModule::waitArrivedPos(QString posName)
{
    XYZModulePos *pos = xyzModuleConfig->getPos<XYZModulePos>(posName);
    m_xAxis->waitArrivedPos(pos->xPos(), pos->xPrecision());
    m_yAxis->waitArrivedPos(pos->yPos(), pos->yPrecision());
    m_zAxis->waitArrivedPos(pos->zPos(), pos->zPrecision());
}
